#include <navigation/utils/local_planner/mover.h>

mover::mover(float max_x_vel, float min_x_vel, float max_x_acc,
             float max_yaw_vel, float min_yaw_vel, float max_yaw_acc,
             float velocity_dec_line_offset_time,
             float velocity_dec_angle_offset_time,
             float recovery_mode_min_dist_timeout,
             float strighting_tolerance, float turning_tolerance,
             float strighting_stop_distance, float turning_stop_distance, float ignore_obstacle_dist,
             float obstacle_acc_ratio,
             float control_freq)
{
    set_kinematic_params(max_x_vel, max_yaw_vel);
    min_x_vel_ = min_x_vel;
    max_x_acc_ = max_x_acc;
    min_yaw_vel_ = min_yaw_vel;
    max_yaw_acc_ = max_yaw_acc;

    velocity_dec_line_offset_time_ = velocity_dec_line_offset_time;
    velocity_dec_angle_offset_time_ = velocity_dec_angle_offset_time;

    recovery_mode_min_dist_timeout_ = recovery_mode_min_dist_timeout;
    recovery_mode_counter_ = 0;

    set_tolerance_params(strighting_tolerance, turning_tolerance);
    turning_tolerance_ = turning_tolerance;
    strighting_stop_distance_ = strighting_stop_distance;
    turning_stop_distance_ = turning_stop_distance;
    ignore_obstacle_dist_ = ignore_obstacle_dist;

    obstacle_acc_ratio_ = obstacle_acc_ratio;

    dist_acc_controller_ = unique_ptr<acc_controller>(new acc_controller());
    angle_acc_controller_ = unique_ptr<acc_controller>(new acc_controller());

    obstalce_state_ = block_state::clear;
    state_ = navigation_state::ready;
    last_state_ = state_;
    strighting_started_ = false;
    turning_started_ = false;
    dist_forward_ = true;
    angle_clockwise_ = true;

    last_dist_diff_ = INFINITY;
    last_angle_diff_ = INFINITY;

    control_freq_ = control_freq;
    vx_delta_ = (1.0f / control_freq_) * max_x_acc_;
    vyaw_delta_ = (1.0f / control_freq_) * max_yaw_acc_;
}

mover::~mover()
{
    if (dist_acc_controller_ != NULL)
    {
        dist_acc_controller_.reset();
    }

    if (angle_acc_controller_ != NULL)
    {
        angle_acc_controller_.reset();
    }
}

void mover::set_kinematic_params(float max_x_vel, float max_yaw_vel)
{
    max_x_vel_ = max_x_vel;
    max_yaw_vel_ = max_yaw_vel;
}

void mover::set_tolerance_params(float strighting_tolerance, float turning_tolerance)
{
    strighting_tolerance_ = strighting_tolerance;
    turning_tolerance_ = turning_tolerance;
}

bool mover::make_plan(geometry_msgs::PoseWithCovarianceStamped pose_init, float dist, float angle, bool ignore_obstacle)
{
    bool ret = false;

    if (state_ == navigation_state::ready)
    {
        pose_init_ = pose_init;
        dist_ = ((abs(dist) <= strighting_tolerance_) ? 0 : dist);
        angle_ = ((abs(angle) <= turning_tolerance_) ? 0 : angle);
        current_dist_ = 0;
        current_angle_ = 0;
        recovery_mode_counter_ = 0;
        last_dist_diff_ = INFINITY;
        last_angle_diff_ = INFINITY;
        dist_forward_ = dist_ >= 0;
        angle_clockwise_ = angle_ <= 0;
        cycle_count_ = 0;
        ignore_obstacle_ = ignore_obstacle;

        twist_target_.linear.x = 0;
        twist_target_.angular.z = 0;

        // 规划距离
        ret |= dist_acc_controller_->make_plan(dist_, max_x_vel_, min_x_vel_, max_x_acc_, velocity_dec_line_offset_time_);

        if (!ret)
        {
            dist_ = 0;
            ret |= dist_acc_controller_->make_plan(dist_, max_x_vel_, min_x_vel_, max_x_acc_, velocity_dec_line_offset_time_);
        }

        // 规划旋转
        ret &= angle_acc_controller_->make_plan(angle_, max_yaw_vel_, min_yaw_vel_, max_yaw_acc_, velocity_dec_angle_offset_time_);

        if (!ret)
        {
            angle_ = 0;
            ret |= dist_acc_controller_->make_plan(angle_, max_yaw_vel_, min_yaw_vel_, max_yaw_acc_, velocity_dec_angle_offset_time_);
        }

        if (ret)
        {
            obstalce_state_ = block_state::clear;
            state_ = navigation_state::processing;
            strighting_started_ = false;
            turning_started_ = false;
        }
    }

    return ret;
}

bool mover::start_session()
{
    bool ret = false;

    if (state_ == navigation_state::processing)
    {
        state_ = navigation_state::strighting;

        ret = true;
    }

    return ret;
}

bool mover::pause_session()
{
    bool ret = false;

    if (state_ != navigation_state::ready &&
        state_ != navigation_state::processing &&
        state_ != navigation_state::reached &&
        state_ != navigation_state::pausing &&
        state_ != navigation_state::blocked)
    {
        last_twist_target_ = twist_target_;

        last_state_ = state_;
        state_ = navigation_state::pausing;

        ret = true;
    }

    return ret;
}

bool mover::continue_session()
{
    bool ret = false;

    if (state_ == navigation_state::pausing)
    {
        state_ = last_state_;

        twist_target_ = last_twist_target_;

        ret = true;
    }

    return ret;
}

navigation_state mover::loop(geometry_msgs::PoseWithCovarianceStamped pose,
                             geometry_msgs::TwistWithCovarianceStamped odom_twist,
                             tecbot_msgs::obstacle obstacle)
{
    if (state_ != navigation_state::ready &&
        state_ != navigation_state::reached &&
        state_ != navigation_state::processing &&
        state_ != navigation_state::pausing)
    {
        // 更新数据
        pose_ = pose;
        odom_twist_ = odom_twist;
        obstacle_ = obstacle;

        if (strighting_started_ || turning_started_)
        {
            obstalce_state_ =
                ((state_ == navigation_state::blocked) ? is_obstacle_detected(
                                                             last_state_before_blocked_)
                                                       : is_obstacle_detected(
                                                             state_));
        }
        else
        {
            obstalce_state_ = block_state::clear;
        }

        // 若为运行模式
        if ((state_ == navigation_state::strighting ||
             state_ == navigation_state::strighting_recovery) &&
            obstalce_state_ == block_state::clear)
        {
            // 计算当前行驶里程
            current_dist_ = (dist_forward_ ? 1.0 : -1.0) * sqrt(pow(pose_.pose.pose.position.x - pose_init_.pose.pose.position.x, 2) +
                                                                pow(pose_.pose.pose.position.y - pose_init_.pose.pose.position.y, 2));

            if (state_ == navigation_state::strighting &&
                (max(abs(twist_target_.linear.x), abs(odom_twist_.twist.twist.linear.x)) > 1e-3 || !strighting_started_) &&
                abs(dist_) > strighting_tolerance_)
            {
                // 计算行驶速度
                twist_target_.linear.x = dist_acc_controller_->get_current_vel(current_dist_);
                twist_target_.angular.z = 0;
                strighting_started_ = true;
            }
            else
            {
                // 超时计时器
                if (abs(odom_twist_.twist.twist.linear.x) >= min_x_vel_ / 2.0f &&
                    abs(dist_ - current_dist_) > last_dist_diff_)
                {
                    recovery_mode_counter_++;
                }
                else
                {
                    recovery_mode_counter_ = 0;
                }

                // 判断是否在容许误差之内
                if (abs(dist_ - current_dist_) <= strighting_tolerance_ ||
                    recovery_mode_counter_ >= (int)abs(recovery_mode_min_dist_timeout_ * control_freq_) ||
                    abs(dist_) <= strighting_tolerance_)
                {
                    // 停车
                    twist_target_.linear.x = 0;
                    twist_target_.angular.z = 0;

                    // 计算初始角度
                    init_angle_ = tf::getYaw(pose_.pose.pose.orientation);
                    last_yaw_ = init_angle_;

                    // 清空计时器
                    recovery_mode_counter_ = 0;

                    // 进入旋转状态
                    state_ = navigation_state::turning;
                }
                else
                {
                    // 判断超出或未到达
                    if (abs(dist_) > abs(current_dist_))
                    {
                        twist_target_.linear.x = (dist_forward_ ? 1 : -1) * min_x_vel_;
                        twist_target_.angular.z = 0;
                    }
                    else
                    {
                        twist_target_.linear.x = (dist_forward_ ? -1 : 1) * min_x_vel_;
                        twist_target_.angular.z = 0;
                    }

                    // 设置恢复模式标记
                    state_ = navigation_state::strighting_recovery;
                }
            }

            // 更新上一次距离
            last_dist_diff_ = abs(dist_ - current_dist_);
        }
        else if ((state_ == navigation_state::turning ||
                  state_ == navigation_state::turning_recovery) &&
                 obstalce_state_ == block_state::clear)
        {
            // 计算当前旋转角度
            double current_yaw = tf::getYaw(pose_.pose.pose.orientation);

            if (last_yaw_ > 0.8 * M_PI && current_yaw < -0.8 * M_PI)
            {
                cycle_count_++;
            }
            else if (last_yaw_ < -0.8 * M_PI && current_yaw > 0.8 * M_PI)
            {
                cycle_count_--;
            }

            current_angle_ = cycle_count_ * 2 * M_PI + current_yaw - init_angle_;

            last_yaw_ = current_yaw;

            if (state_ == navigation_state::turning &&
                (max(abs(twist_target_.angular.z), abs(odom_twist_.twist.twist.angular.z)) > 1e-3 || !turning_started_) &&
                abs(angle_) > turning_tolerance_)
            {
                // 计算行驶速度
                twist_target_.angular.z = angle_acc_controller_->get_current_vel(current_angle_);
                twist_target_.linear.x = 0;
                turning_started_ = true;
            }
            else
            {
                // 超时计时器
                if (abs(odom_twist_.twist.twist.angular.z) >= min_yaw_vel_ / 2.0f &&
                    abs(angle_ - current_angle_) > last_angle_diff_)
                {
                    recovery_mode_counter_++;
                }
                else
                {
                    recovery_mode_counter_ = 0;
                }

                // 判断是否在容许误差之内
                if (abs(angle_ - current_angle_) <= turning_tolerance_ ||
                    recovery_mode_counter_ >= (int)abs(recovery_mode_min_dist_timeout_ * control_freq_) ||
                    abs(angle_) <= turning_tolerance_)
                {
                    // 停车，恢复状态
                    twist_target_.linear.x = 0;
                    twist_target_.angular.z = 0;

                    obstalce_state_ = block_state::clear;
                    state_ = navigation_state::reached;
                }
                else
                {
                    // 判断超出或未到达
                    if (abs(angle_) > abs(current_angle_))
                    {
                        twist_target_.angular.z = (angle_clockwise_ ? -1 : 1) * min_yaw_vel_;
                        twist_target_.linear.x = 0;
                    }
                    else
                    {
                        twist_target_.angular.z = (angle_clockwise_ ? 1 : -1) * min_yaw_vel_;
                        twist_target_.linear.x = 0;
                    }

                    // 设置恢复模式标记
                    state_ = navigation_state::turning_recovery;
                }
            }

            // 更新上一次角度
            last_angle_diff_ = abs(angle_ - current_angle_);
        }
        else if (state_ == navigation_state::blocked &&
                 obstalce_state_ == block_state::clear)
        {
            twist_target_ = last_twist_target_;
            vx_delta_ /= obstacle_acc_ratio_;
            vyaw_delta_ /= obstacle_acc_ratio_;
            state_ = last_state_before_blocked_;
        }
        else if (state_ != navigation_state::blocked &&
                 obstalce_state_ != block_state::clear)
        {
            last_twist_target_ = twist_target_;
            vx_delta_ *= obstacle_acc_ratio_;
            vyaw_delta_ *= obstacle_acc_ratio_;
            twist_target_.linear.x = 0;
            twist_target_.angular.z = 0;
            last_state_before_blocked_ = state_;
            state_ = navigation_state::blocked;
        }
    }
    else if (state_ == navigation_state::pausing)
    {
        twist_target_.linear.x = 0;
        twist_target_.angular.z = 0;
    }

    return state_;
}

geometry_msgs::Twist mover::get_current_twist()
{
    if (twist_target_.linear.x > odom_twist_.twist.twist.linear.x)
    {
        if (!dist_forward_ &&
            (state_ == navigation_state::strighting || state_ == navigation_state::strighting_recovery))
        {
            // 如果为运行模式，且为减速方向，则跟随速度
            twist_.linear.x = twist_target_.linear.x;
        }
        else
        {
            // 否则严格按照运动学进行加速
            twist_.linear.x += vx_delta_;

            if (twist_.linear.x > twist_target_.linear.x)
            {
                twist_.linear.x = twist_target_.linear.x;
            }
        }
    }
    else if (twist_target_.linear.x < odom_twist_.twist.twist.linear.x)
    {
        if (dist_forward_ &&
            (state_ == navigation_state::strighting || state_ == navigation_state::strighting_recovery))
        {
            // 如果为运行模式，且为减速方向，则跟随速度
            twist_.linear.x = twist_target_.linear.x;
        }
        else
        {
            // 否则严格按照运动学进行加速
            twist_.linear.x -= vx_delta_;

            if (twist_.linear.x < twist_target_.linear.x)
            {
                twist_.linear.x = twist_target_.linear.x;
            }
        }
    }

    if (twist_target_.angular.z > odom_twist_.twist.twist.angular.z)
    {
        if (angle_clockwise_ &&
            (state_ == navigation_state::turning || state_ == navigation_state::turning_recovery))
        {
            // 如果为运行模式，且为减速方向，则跟随速度
            twist_.angular.z = twist_target_.angular.z;
        }
        else
        {
            // 否则严格按照运动学进行加速
            twist_.angular.z += vyaw_delta_;

            if (twist_.angular.z > twist_target_.angular.z)
            {
                twist_.angular.z = twist_target_.angular.z;
            }
        }
    }
    else if (twist_target_.angular.z < odom_twist_.twist.twist.angular.z)
    {
        if (!angle_clockwise_ &&
            (state_ == navigation_state::turning || state_ == navigation_state::turning_recovery))
        {
            // 如果为运行模式，且为减速方向，则跟随速度
            twist_.angular.z = twist_target_.angular.z;
        }
        else
        {
            // 否则严格按照运动学进行加速
            twist_.angular.z -= vyaw_delta_;

            if (twist_.angular.z < twist_target_.angular.z)
            {
                twist_.angular.z = twist_target_.angular.z;
            }
        }
    }

    return twist_;
}

float mover::get_current_dist()
{
    return current_dist_;
}

float mover::get_current_angle()
{
    return current_angle_;
}

block_state mover::get_obstacle_state()
{
    return obstalce_state_;
}

void mover::reset_state()
{
    twist_target_.linear.x = 0;
    twist_target_.angular.z = 0;
    strighting_started_ = false;
    turning_started_ = false;
    ignore_obstacle_ = false;
    state_ = navigation_state::ready;
}

block_state mover::is_obstacle_detected(navigation_state state)
{
    block_state ret = block_state::clear;

    if (state == navigation_state::strighting ||
        state == navigation_state::strighting_recovery)
    {
        if (dist_forward_)
        {
            for (int i = 0; i < obstacle_.size; i++)
            {
                if (obstacle_.obstacle_front[i] > 0 &&
                    obstacle_.obstacle_front[i] < (ignore_obstacle_ ? ignore_obstacle_dist_ : strighting_stop_distance_) &&
                    obstacle_.obstacle_front[i] < abs(dist_ - current_dist_))
                {
                    ret = block_state::forward;
                    break;
                }
            }
        }
        else
        {
            for (int i = 0; i < obstacle_.size; i++)
            {
                if (obstacle_.obstacle_rear[i] > 0 &&
                    obstacle_.obstacle_rear[i] < (ignore_obstacle_ ? ignore_obstacle_dist_ : strighting_stop_distance_) &&
                    obstacle_.obstacle_rear[i] < abs(dist_ - current_dist_))
                {
                    ret = block_state::backward;
                    break;
                }
            }
        }
    }
    else if (state == navigation_state::turning ||
             state == navigation_state::turning_recovery)
    {
        if (!angle_clockwise_)
        {
            for (int i = 0; i < obstacle_.size; i++)
            {
                if (obstacle_.obstacle_left[i] > 0 &&
                    obstacle_.obstacle_left[i] < (ignore_obstacle_ ? ignore_obstacle_dist_ : turning_stop_distance_))
                {
                    ret = block_state::left;
                    break;
                }
            }
        }
        else
        {
            for (int i = 0; i < obstacle_.size; i++)
            {
                if (obstacle_.obstacle_right[i] > 0 &&
                    obstacle_.obstacle_right[i] < (ignore_obstacle_ ? ignore_obstacle_dist_ : turning_stop_distance_))
                {
                    ret = block_state::right;
                    break;
                }
            }
        }
    }

    return ret;
}